'' Octopilot -- Octopilot.spin
'' (c) 2008-2009 Ben Levitt
'' http://code.google.com/p/octopilot/
''
'' This is the Top Level spin file for the Octopilot program
'' It sets up the other objects and loops over the control objects (Navigation, Stabilization, and Action)
'' each time there are new servo values from the receiver. (via the ServoIO object)


' Cog Usage
'   Autopilot.ServoIOLoop
'   Autopilot.AutoPilot
'   AutoPilot.Debug
'   Stabilization.IMU.go
'   Stabilization.IMU.MCP
'   Navigation.readNMEA
'   Navigation.SerialGps
'   VideoOut.TV_Text


OBJ
  config        : "Config"
  servo         : "ServoIO"
  stabilization : "Stabilization"
  navigation    : "Navigation"
  action        : "Action"            ' Action will be a simple control object that pulses a pin when reaching a waypoint
  video         : "VideoOut"
  'debug         : "FullDuplexSerial"


CON
  _CLKMODE                      = XTAL1 + PLL16X
  _XINFREQ                      = 5_000_000


VAR
  ' GlobalVars
  long inServoValsAddr          ' address of a word array
  long outServoValsAddr         ' address of a word array
  long defaultServoValsAddr     ' address of a word array
  long dataInReadyFlagAddr      ' address of a byte
  long dataOutReadyFlagAddr     ' address of a byte

  byte navState
  byte lastAutoEnabledVal


PUB Start | i
  dataInReadyFlagAddr := 0
  dataOutReadyFlagAddr := 0

  ' Initialize ServoIO
  servo.Start(config#NumInChannels, config#NumOutChannels, config#inServoBase, config#outServoBase, config#servoLedPin, config#throttle, config#throttleLowValue)
  ' Get addresses of the ServoIO data, returned by reference
  servo.GetAddresses(@inServoValsAddr, @outServoValsAddr, @defaultServoValsAddr, @dataInReadyFlagAddr, @dataOutReadyFlagAddr)
  
  ' Initialize module objects
  if config#navigationEnabled
    navigation.Start(inServoValsAddr, outServoValsAddr, defaultServoValsAddr, @navState)
  if config#stabilizationEnabled
    stabilization.Start(inServoValsAddr, outServoValsAddr, defaultServoValsAddr, @navState)
  if config#actionEnabled
    action.Start(inServoValsAddr, outServoValsAddr, defaultServoValsAddr, @navState)
  if config#videoOutEnabled
    video.Start(inServoValsAddr, outServoValsAddr, defaultServoValsAddr, @navState)
  
  'debug.Start(config#debugPinBase+1, config#debugPinBase, 0, 38400)
  
  repeat
    ' Set default vals if we haven't yet
    servo.CheckForNewDefaultValues

    ' Sync to servo cycle
    repeat until BYTE[dataInReadyFlagAddr] <> 0
    BYTE[dataInReadyFlagAddr] := 0
    
    ' If Autopilot is enabled, apply stabilization, navigation, etc.
    if autopilotEnabled
    
      ' Tell Stabilization that we're switching back to autopilot mode
      if lastAutoEnabledVal == 0
        lastAutoEnabledVal := 1
        stabilization.EnabledAutopilot
      
      if config#navigationEnabled
        navigation.Apply
      
      if config#stabilizationEnabled
        stabilization.Apply
      
      if config#actionEnabled
        action.Apply
      
      if WORD[inServoValsAddr+config#throttle*2] => config#throttleLowValue
        ' Pass the throttle in val through to the out val if it's valid (above threshold)
        WORD[outServoValsAddr+config#throttle*2] := WORD[inServoValsAddr+config#throttle*2]
      else 
        ' Don't allow sending servo values below the bottom of the throttle-stick's current setup
        WORD[outServoValsAddr+config#throttle*2] := WORD[defaultServoValsAddr+config#throttle*2]
      
    ' Else, if autopilot is disabled
    else
      repeat i from 0 to config#NumOutChannels-1
        ' Only update the out values if they have changed by more than 9/10000 of the range to avoid jitter
        if ||(WORD[outServoValsAddr+i*2] - WORD[inServoValsAddr+i*2]) > 9
          WORD[outServoValsAddr+i*2] := WORD[inServoValsAddr+i*2]
      
      lastAutoEnabledVal := 0
      
    ' Tell servo code that we're done setting servo values
    ' If this isn't set in time for the next servo pulse, old data is used
    ' If data isn't set for 25 servo pulses in a row (500mS) the ServoIO cog
    ' switches back to using the input data from the receiver (i.e. manual control)
    ' instead of using output data from the stabilization/navigation code.
    
    BYTE[dataOutReadyFlagAddr] := 1
    
    ' Modules that don't change outServoVals should hapen here, after dataOutputReadyFlag is set,
    ' since servo output doesn't need to wait on these modules' Apply processing.
    if config#videoOutEnabled
      video.Apply
    
    
    'PrintInts(inServoValsAddr, config#NumInChannels)                            ' show servo inputs
    'debug.str(string("                                           "))
    'PrintInts(outServoValsAddr, config#NumOutChannels)                          ' show servo outputs


PRI autopilotEnabled
  if servo.GetSwitchState(config#autopilotSwitch, 2) == 1
    return 1
  
  ' Check for throttle failsafe position, since by default the throttle is the only channel that drops back to
  ' the failsafe position on signal loss.  To make this work, we bind the receiver while the transmitter's
  ' throttle is set to -150%, which sets the receiver's throttle failsafe value low.  Then we set the transmitter's
  ' throttle minimum back to -100%, so that the failsafe value is easily distinguished from a real low throttle signal.
  if servo.IsFailsafeBelowThreshold == 1
    return 1

  ' If neither, then return 0
  return 0


{{
PRI PrintInts(vals, count) | i
  repeat i from 0 to count-1
    PrintInt(WORD[vals+i*2])
  debug.tx(13)
  'debug.tx(10) 


'Print out Num/10, left-padded with a "-" or a space, and with zeros to be 4 digits long, plus a comma and a space
PRI PrintInt(Num)
  if Num < 0
    debug.tx("-")
    Num := 0 - Num
  else
    debug.tx(" ")
  
  debug.tx((Num / 10000) + "0") 
  debug.tx(((Num / 1000) // 10) + "0") 
  debug.tx(((Num / 100) // 10) + "0") 
  debug.tx(((Num / 10) // 10) + "0") 
  debug.tx(",") 
  debug.tx(" ")
}}
 